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Combining  Collision  Avoidance  and  Operator  Workload  Reduction  with 
Cooperative  Task  Assignment  and  Path  Planning 


Jeffery  B.  Saunders  Steven  J.  Rasmussen  Corey  J.  Schumacher 
Control  Science  Center  of  Excellence 
Air  Force  Research  Laboratory 
Wright-Patterson  AFB,  OH,  45433 


Abstract — This  paper  develops  a  method  of  assignment  and 
path  allocation  that  incorporates  a  priori  collision  avoidance 
and  operator  workload  reduction  in  assigning  multiple  tasks  to 
cooperative  unmanned  aerial  vehicles  (UAV).  The  problem  is 
posed  as  a  combinatorial  optimization  problem.  A  branch  and 
bound  tree  search  algorithm  is  implemented  for  a  satisficing 
solution  using  a  cost  function  that  integrates  distance  travelled, 
proximity  to  other  UAVs,  and  target  visitation  times.  The 
results  demonstrate  that  the  assigned  path  is  near  optimal 
with  respect  to  distance  travelled,  significantly  increases  the 
expected  proximity  distance  to  other  UAVs,  and  significantly 
increases  the  difference  between  visitation  times  of  targets.  The 
algorithm  runs  in  less  than  a  tenth  of  a  second  allowing  on  the 
fly  replanning. 

Introduction 

Micro  aerial  vehicles  (MAVs)  have  received  increasing 
amounts  of  attention  in  the  last  few  years.  Low  cost  and 
simple  operation  give  them  application  for  military  and 
civilian  purposes  alike.  Small  onboard  autopilots  allow  for 
accurate  autonomous  navigation  removing  the  need  to  place 
pilots  in  danger.  The  next  logical  step  is  deployment  of  teams 
of  MAVs  to  cooperatively  and  synergistically  accomplish  a 
series  of  tasks. 

A  cooperative  team  of  MAVs  has  the  potential  to  produce 
greater  and  more  efficient  results  than  a  team  of  independent 
MAVs.  They  can  adapt  to  changing  situations,  replan  task 
assignments  when  a  MAV  is  lost,  communicate  over  large 
areas,  and  quickly  complete  scenarios.  We  would  like  to  use 
as  much  of  that  potential  as  possible.  The  goal  of  the  work 
documented  in  this  paper  is  to  produce  a  method  of  task 
assignment  such  that  we  can  produce  a  flight  plan  a  priori  and 
potentially  replan  mid-flight  to  adapt  to  changing  situations. 

Many  military  intelligence  and  reconnaissance  mission 
scenarios  involving  teams  of  MAVs  have  been  introduced 
in  the  past  few  years  [1].  Each  scenario  involves  varying 
degrees  of  coupling  between  the  goals  of  team  coordination, 
task  priority,  and  planning  feasible  paths  to  accomplish  tasks. 
Finding  a  general  optimal  solution  is  difficult.  Many  methods 
result  in  combinatorial  problems  that  are  difficult  to  solve. 
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Algorithms  in  [2]  and  [3]  use  mixed  integer  linear  pro¬ 
gramming  to  solve  the  combinatorial  problem.  Both  methods 
involve  a  high  amount  of  computation  and  cannot  integrate 
other  coupled  factors  without  adding  to  computation  time. 
The  capacitated  transhipment  network  solver  in  [4]  is  unable 
to  generate  multiple  task  assignments.  The  iterated  capac¬ 
itated  transhipment  network  solver  in  [5]  is  heuristic  in 
nature,  terminates  after  finding  an  initial  solution,  and  can 
not  guarantee  an  optimal  solution. 

The  desired  solution  must  have  the  ability  couple  all 
constraining  aspects  of  the  problem,  and  be  expandable  to 
include  other  desired  constraints.  The  branch  and  bound  tree 
search  described  in  [6]  is  expandable  to  include  additional 
constraints.  We  propose  to  improve  the  algorithm  to  include 
additional  costs  to  compliment  the  distance  cost  already 
introduced  in  [6] .  We  will  show  that  the  combined  additional 
costs  produce  near  optimal  results  while  successfully  adding 
needed  constraints. 

Ref  [7]  describes  a  research  program  called  Coopera¬ 
tive  Operations  in  Urban  Terrain  (COUNTER).  For  the 
COUNTER  concept,  a  small  unmanned  aerial  vehicle  (SAV) 
transports  MAVs  to  an  area  of  interest,  usually  an  urban  area. 
The  SAV  flies  over  the  urban  terrain,  or  some  designated 
area,  and  releases  battery  powered  MAVs  to  observe  objects 
of  interest  at  closer  proximity.  The  MAVs  are  released  at 
points  along  the  path  of  the  SAV  after  which  the  MAVs  fly  to 
various  objects  and  transmit  video  footage  of  the  objects  back 
to  an  operator.  The  MAVs  cooperate  together  to  observe  all 
the  objects  in  a  timely  manner  after  which  they  either  return 
to  a  recover  point  or  continue  to  another  assigned  point.  The 
objective  of  the  scenario  is  to  view  all  objects  as  quickly  as 
possible.  One  concern  is  that  all  of  the  MAVs  may  visit  their 
assigned  objects  near  the  same  time  which  will  over-load  the 
operator.  Another  concern  is  that  the  MAVs  have  no  method 
of  avoiding  collisions  with  each  other.  This  paper  addresses 
both  of  these  concerns  through  the  introduction  of  new  costs 
in  the  task  allocation  and  path  optimization  algorithm. 

The  organization  of  this  paper  is  as  follows.  The  notation 
used  in  the  paper  is  described  in  section  I.  A  full  description 
of  the  proposed  scenario  is  in  section  II  followed  by  a 
review  of  the  branch  and  bound  tree  search  in  section  III. 
Sections  IV  and  V  describe  the  collision  prediction  algorithm 
and  simulation  environment  with  a  review  of  the  results  in 
section  VI.  We  conclude  with  some  points  of  interest  in 
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section  VII. 

I.  Notation 

Here  we  present  the  notation  that  will  be  used  throughout 
the  rest  of  the  paper.  This  is  intended  to  be  a  reference  for 
the  rest  of  the  text. 

r i  -  Position  of  the  ith  MAV. 

R i  -  Total  distance  travelled  by  the  ith  MAV. 

R  -  MAV  turn  radius. 

U  -  The  set  of  all  MAVs. 

Vi  -  Velocity  (groundspeed)  of  the  ith  MAV. 

Vie  -  Velocity  (groundspeed)  max  error  of  the  ith 
MAV. 

Wij  -  Waypoint  j  of  the  ith  MAV. 
rd  -  Two  dimensional  safety  radius  of  a  MAV.  No 
MAV  should  be  less  than  from  other  MAVs. 
dij  -  The  minimum  distance  between  MAVs  i  and 
3- 

II.  Problem  Description 

The  motivation  for  this  work  is  to  develop  algorithms  to 
enhance  the  COUNTER  solution  for  intelligence  and  recon¬ 
naissance  missions  [7].  The  COUNTER  research  program 
proposes  to  have  an  operator  control  one  SAV  and  four 
MAVs  as  they  perform  the  mission.  The  mission  is  to  find 
and  investigate  operator  selected  objects,  suspected  targets, 
in  the  city  in  order  to  identify  targets.  The  task  for  the  SAV 
is  to  orbit  above  the  city  providing  video  to  the  operator  and 
acting  as  a  relay  for  the  MAVs.  The  MAVs  are  tasked  to  visit 
the  selected  objects  in  the  city  in  order  to  convict  or  acquit 
those  objects  as  being  targets.  Since  there  are  many  UAVs 
involved  and  only  one  operator,  the  algorithms  developed  for 
COUNTER  were  designed  to  utilize  the  MAVs  to  maximize 
the  number  of  objects  acquitted  or  convicted. 

COUNTER  is  a  cooperative  control  environment  in  which 
MAVs  must  cooperate  together  to  quickly  observe  objects. 
The  cooperative  environment  involves  many  coupled  prob¬ 
lems  that  may  not  be  so  obvious  at  first  glance  includ¬ 
ing  minimizing  distance  travelled,  collision  avoidance,  stag¬ 
gered  target  arrival  (to  optimize  operator  efficiency),  and 
revisiting  poorly  observed  targets.  To  perform  this  mission 
COUNTER’S  cooperative  control  algorithms  are  divided  into 
two  parts,  object  allocation/path  planning  and  object  vis¬ 
itation  control.  This  paper  attempts  to  solve  the  coupling 
of  minimizing  distance  travelled,  collision  avoidance,  and 
staggered  target  arrival.  Other  coupled  parts  of  the  problem 
may  also  be  solvable  by  the  same  methods,  but  are  not 
explored  here. 

III.  Branch  and  Bound  Algorithm 

The  Branch  and  Bound  tree  search  is  described  fully  in 
[6]  and  is  presented  here  for  completeness. 

Combinatorial  problems  can  be  completely  represented  by 
a  decision  tree.  A  decision  tree  is  defined  by  a  set  of  nodes 
T  =  (N,E)  where  N  is  the  set  of  all  nodes  and  E  is  the 
set  of  all  edges.  Each  edge  is  directed,  meaning  it  can  only 
be  traversed  in  one  direction.  Every  node  can  have  parent 


nodes  from  which  an  edge  is  traversed,  or  children  node 
to  which  an  edge  is  traversed.  The  root  node  is  the  single 
node  without  a  parent  node  from  which  all  nodes  derive. 
Leaf  nodes  are  nodes  without  children  nodes.  The  root  node 
begins  the  tree  and  the  leaf  nodes  terminate  the  tree.  The 
leaf  nodes  represent  possible  solutions. 

Each  node  in  the  tree  is  given  a  cost  based  on  the  scenario 
it  represents.  The  cost  function  determines  which  solutions 
are  optimal.  The  leaf  node  with  the  lowest  cost  is  considered 
the  optimal  solution. 

Branch  and  bound  assumes  the  cost  of  nodes  monoton- 
ically  increases,  i.e.  all  children  of  a  node  have  a  higher 
cost  than  the  node  itself.  Using  a  best  first  search,  a  feasible 
solution  can  quickly  be  found.  Any  nodes  with  a  larger 
cost  than  the  initial  solution  can  be  pruned  from  the  tree 
significantly  reducing  the  number  of  nodes  to  expand.  Large 
parts  of  the  tree  can  be  pruned.  If  a  new  leaf  node  with  a 
lower  cost  is  found,  it  becomes  the  current  optimal  solution 
with  which  to  compare  other  nodes.  The  search  can  continue 
till  either  the  tree  is  completely  searched  and  the  optimal 
solution  is  found,  or  some  time  limit  is  reached  at  which  the 
current  best  solution  is  used. 

In  the  COUNTER  scenario,  the  root  node  represents  all 
the  vehicles  and  targets  without  any  assignments.  Each 
edge  traversal  adds  one  target  assignment  to  a  vehicle.  The 
leaf  nodes  represent  complete  sets  of  target  assignments  to 
vehicles.  In  [6],  the  cost  of  a  node  is  the  maximum  distance 
flown  by  any  one  MAV.  The  optimal  solution  minimizes  the 
maximum  distance  flown  by  any  one  MAV. 

IV.  Cost  Function  Algorithm 

Our  goal  is  to  find  a  set  of  target  assignments  that 
minimize  the  distance  travelled  by  any  of  the  vehicles  while 
maintaining  a  large  separation  distance  between  MAVs  and 
staggering  the  MAVs  arrival  at  objects.  The  J2  (min-max) 
cost  function  of  [6] 

J2  =  maxRi  >  0  (1) 

ieu 

is  used  to  minimize  the  distance  travelled  by  any  of  the 
MAVs.  In  order  to  meet  the  other  objectives  of  the  goal, 
we  will  add  one  cost  representing  the  proximity  distance 
to  other  MAVs  (collision  cost)  and  another  representing  the 
minimum  time  between  target  arrivals  (arrival  time  cost). 

A.  Collision  Cost 

We  assume  all  paths  flown  by  MAVs  between  targets  are 
Dubbin’s  paths  [8].  Each  path  consists  of  turns  of  radius 
R  and  straight  lines.  Lines  and  turns  will  be  considered 
different  waypoints  to  simplify  calculations  even  though  they 
represent  a  single  Dubbin’s  path.  The  following  descriptions 
estimate  the  minimum  distance  between  MAVs  flying  com¬ 
binations  of  straight  line  paths  and  turning  paths. 

1)  Two  Straight  Line  Paths:  Straight  line  paths  can  be 
described  parametrically  by: 

m i  =  w ij  -  Wij&b 
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m, 


into 


m i  _  w jj  -  Wjjm 

II  il  wi,i  -  wi,i- 1  il  ’ 

and 

r*  =  Vitnii  +  Wij-i,  (2) 

where  j  is  the  current  waypoint  and  t  =  0  when  77  =  Wij- i. 
Equation  2  represents  a  MAV  perfectly  tracking  a  waypoint 
path  starting  at  Wij-i  when  £  =  0  and  finishing  at  w ij 
when  t  = 

We  want  to  know  the  distance  between  two  MAVs  at  any 
point  along  the  path, 

II  rx-r2  ||2=  d 2, 

||  ( \  i / in i  +  w,.,  i  -  V2tm2  -  w2,j_i)  ||2=  d2.  (3) 

and  take  the  minimum.  The  minimum  can  be  found  by  taking 
the  derivative  and  setting  equal  to  zero.  The  result  is: 

_  Vimf  (w2,j-i  -  wij_i)  +  V2m% (wij_i  -  w2,j_i) 
+  V2  m'^  mo  —  2V’i  VomJ"  m2 

(4) 

The  minimum  distance  can  be  found  by  substituting  t  into 

d  =||  ri  —  r2  ||  -  (5) 

2)  Turn  and  Straight  Line  Path:  The  minimum  distance 
between  a  turn  and  a  straight  path  is  somewhat  more  difficult 
algebraically.  A  gradient  descent  could  be  useful  for  accu¬ 
racy,  but  introduces  a  large  amount  of  computation.  We  can 
estimate  the  proximity  of  a  turn  path  and  a  straight  path  to 
within  an  error  of  2  R  by  finding  the  geometric  intersections 
of  the  paths  and  checking  the  times  of  those  intersections. 
Consider  the  equations  of  a  circle  and  line: 

(re  -  Wi,j-I,e)2  +  (rn  -  wltj-it„)2  =  (R  +  rd )2,  (6) 

and 


(ID 


rn  can  be  found  by  substituting  re  back  into  equation  7.  If 
the  result  is  imaginary,  then  the  circle  and  line  don’t  intersect. 
If  the  intersection  isn’t  in  the  time  frame  of  the  waypoints, 
then  they  don’t  intersect.  In  these  cases,  the  distance  from 
the  center  of  the  circle  to  the  MAV  on  the  waypoint  path  in 
the  correct  time  frame  can  estimate  the  distance. 

3)  Two  Turns:  For  the  distance  between  two  turns,  the 
distance  between  the  circles  is  sufficient  if  they  are  in  the 
same  time  frame.  This  introduces  a  maximum  error  of  4 R. 

4)  Cost  Function:  The  cost  function  needs  an  adjustable 
parameter  such  that  a  desired  minimum  distance  is  imposed 
on  the  MAVs.  An  inverse  square  distance  relationship  allows 
the  cost  to  greatly  increase  as  distance  decreases.  Multiplica¬ 
tion  by  [3  adds  a  tunable  parameter  to  influence  the  distance 
at  which  the  cost  becomes  significant.  The  collision  cost 
function  is, 


Jcc  —  .  .2  >  0,  {i,  j}  £  t/,  i  7^  T  (12) 

mmz,j  (dij) 

B.  Arrival  Time  Cost 

The  arrival  time  at  each  target  is  calculated  using  the 
length  of  the  path  travelled  to  the  target  and  a  constant  veloc¬ 
ity  assumption.  We  find  the  minimum  difference  between  any 
two  arrival  times  and  substitute  that  into  an  inverse  square 
formula  for  the  cost.  Once  again  we  multiply  the  result  by 
a  constant  7  to  tune  the  cost.  The  arrival  time  cost  is, 


J  AT  =  — - 3}  (13) 

mm  ij  (tij) 

where  Uj  is  the  difference  in  arrival  time  between  targets  i 
and  j. 

C.  Combined  Arrival  Time  and  Collision  Cost 

The  combined  cost  function  combines  the  min-max  cost 
with  the  two  new  costs  into  one  function.  The  constants  f3 
and  7  in  Eq  12  and  Eq  13  allow  for  tuning.  The  function  is, 


rn  =  m(re  -  w2,j- i,e)  +  w2,j*i,n-  (7) 

These  can  be  simultaneously  solved  and  the  minimum  can 
be  found  using  the  quadratic  equation.  Substitute 

a  =  1  +  m2,  (8) 

b  =  2(w2,j-l,n  -  Wi,j-l,n  -  mw2J-1:e)  -  2wij-1>e,  (9) 

and 

C  =  -  Wij-1  -  mw 2,j-l,e)2 

-<iW  ,10) 


J  =  J2  +  Jcc  +  J  AT  (14) 

V.  Simulation 

We  ran  100  Monte  Carlo  simulations  to  demonstrate  the 
effectiveness  of  the  collision  cost,  arrival  time  cost,  and 
combined  cost.  Each  run  consists  of  three  vehicles  and  seven 
targets.  The  number  of  targets  and  vehicles  is  small  to  allow 
time  to  search  the  tree  for  the  optimal  solution  in  each  run. 
The  optimal  solution  could  then  be  compared  with  the  time 
limited  branch  and  bound  solution. 

Each  run  used  a  uniform  distribution  to  place  targets 
and  vehicle  starting  positions  on  square  map.  The  map  size 
is  3,000  feet  by  3,000  feet  for  all  calculations.  However, 
simulations  were  also  run  on  maps  of  up  to  7,000  feet  by 
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Minimum  distance  between  MAVs  in  a  tour 


Fig.  L  The  minimum  distance  between  MAVs  over  a  100  tours  is  shown. 


Percentage  Increase  in  Minimum  Distance 
from  Min-Max  Run  to  Collision  Cost  Run 


Fig.  2.  The  percentage  increase  in  the  minimum  distance  from  the  min-max 
cost  run  to  the  collision  cost  run. 

7,000  feet  to  show  the  path  planner  effectiveness  at  larger 
map  sizes.  Each  MAV  is  assumed  to  have  the  same  constant 
velocity. 

The  path  planner  was  executed  to  the  optimal  solution 
four  times  for  each  run,  once  for  min-max  cost,  collision 
cost,  arrival  time  cost,  and  combined  cost.  With  each  cost 
function  computed  on  the  same  data,  they  could  be  compared 
with  more  accuracy.  We  chose  for  parameters  (3  =  1000000 
and  7  =  15000. 

VI.  Results 

A.  Collision  Cost 

Our  goal  with  the  collision  cost  was  to  increase  the 
proximity  distance  of  the  MAVs  with  minimal  change  in  the 
min-max  cost.  The  distance  between  MAVs  was  increased 
with  an  average  of  90%  compared  to  the  min-max  cost  runs. 
The  average  increase  in  min-max  cost  was  0.8%. 


Difference  in  Min-Max  Cost 


Fig.  3.  The  difference  in  the  min-max  costs  between  the  min-max  run  and 
the  collision  cost  run. 


Average  Minimum  Distances  for  Each  Cost  Function 


Fig.  4.  The  average  minimum  distance  between  MAVs  as  nodes  are 
processed  is  shown.  The  collision  cost  results  in  increased  distance  between 
MAVs. 

Figure  1  shows  the  distance  increase  for  each  of  the  100 
runs  with  the  percentage  increase  in  figure  2.  The  runs  are 
ordered  with  increasing  distance.  Most  of  the  runs  result  in  a 
significant  increase  in  distance.  The  runs  without  change  in 
distance  already  have  a  large  distance  between  MAVs,  with 
the  smallest  at  300  feet. 

The  difference  in  min-max  cost  between  the  two  runs 
is  displayed  in  figure  3.  Little  difference  is  made  in  min- 
max  cost  between  cost  functions.  Notice  the  two  spikes  in 
the  graph.  This  is  the  result  of  two  MAVs  with  starting 
positions  in  close  proximity.  If  the  MAVs  have  similar 
starting  positions,  the  planner  will  remove  one  of  them 
from  flight  to  prevent  collision.  This  is  expected,  however 
undesirable. 

All  of  the  above  calculations  result  from  tree  searches  to 
find  the  optimal  solution.  The  optimal  solution  can  take  large 
periods  of  time  to  find  due  to  the  combinatorial  nature  of  the 
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Fig.  5.  The  minimum  difference  in  arrival  times  between  targets  is  plotted 
for  each  run.  The  arrival  time  cost  function  produces  arrival  time  differences 
better  than  or  equal  to  the  min-max  cost  function. 

problem.  However,  the  branch  and  bound  search  can  find  a 
near  optimal  solution  in  tenths  of  a  second.  Figure  4  shows 
the  increase  in  average  minimum  distance  across  the  runs 
as  nodes  are  processed.  A  larger  amount  of  nodes  implies 
a  larger  amount  of  time  to  process.  The  minimum  distance 
from  the  collision  cost  runs  tends  to  increase  as  more  nodes 
are  processed.  Notice  the  minimum  distance  in  the  min- 
max  costs  runs  show  little  change  over  number  of  processed 
nodes. 

B.  Arrival  Time  Cost 

The  arrival  time  cost  has  similar  results  to  the  collision 
cost.  Figure  5  shows  the  arrival  time  difference  between  the 
arrival  time  cost  function  and  the  min-max  cost  function.  The 
arrival  time  cost  function  results  in  arrival  time  differences 
greater  than  or  equal  to  that  of  the  mix-max  cost  function. 
Arrival  time  differences  are  increased  by  133%.  The  min- 
max  cost  is  increased  by  0.06%. 

The  arrival  time  difference  is  plotted  with  the  nodes 
processed  in  figure  6.  Once  again,  they  are  similar  to  the 
results  of  the  collision  cost.  The  initial  solution  has  an  initial 
increase  in  arrival  time  difference  and  the  solution  improves 
over  time. 

C.  Combined  Cost 

The  combined  cost  has  the  potential  to  improve  proximity 
of  the  aircraft  and  stagger  target  arrival  times  while  keeping 
the  distance  flown  by  the  MAVs  close  to  optimal.  The 
performance  of  these  goals  can  be  measured  by  comparing 
the  results  of  the  combined  cost  function  to  that  of  the  other 
cost  functions. 

Figure  7  compares  the  expected  proximity  distance  of  the 
MAVs  as  nodes  are  processed.  The  combined  cost  function 
results  are  similar  to  the  results  of  the  collision  cost  function. 
Adding  another  cost  had  very  little  effect  on  proximity 
results. 


Nodes  Processed  for  Assignment 


Fig.  6.  The  minimum  difference  in  arrival  times  between  targets  is  plotted 
against  number  of  nodes  processed. 


Fig.  7.  The  minimum  expected  distance  between  MAVs  is  plotted  against 
number  of  nodes  processed.  The  results  for  the  combined  cost  function  are 
very  similar  to  the  results  of  the  collision  cost  function. 


The  differences  in  target  arrival  times  are  shown  in  fig¬ 
ure  8.  The  combined  cost  function  results  are  similar  to  the 
arrival  time  cost  function.  The  combined  cost  has  done  little 
to  negatively  affect  the  target  arrival  time  difference. 

The  min-max  cost  increased  by  0.64%  from  the  min- 
max  cost  run  to  the  combined  cost  run.  This  is  a  small 
difference  and  worth  the  large  increase  in  proximity  distance 
and  staggered  target  arrival. 

VII.  Conclusion 

In  this  paper  we  have  demonstrated  a  method  of  assigning 
targets  to  multiple  vehicles  while  balancing  objectives  of 
distance  travelled,  collision  avoidance,  and  staggered  target 
arrival.  The  results  show  significantly  improved  expected  dis¬ 
tance  between  MAVs  and  a  large  increase  in  staggered  arrival 
time.  Both  of  these  features  may  be  added  simultaneously 
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solutions  that  may  satisfy  a  variety  of  constraints. 


Fig.  8.  The  expected  minimum  difference  in  arrival  times  is  plotted  against 
number  of  nodes  processed.  The  arrival  time  and  combined  cost  functions 
have  similar  results. 

with  little  difference  in  results.  Suboptimal  solutions  may  be 
found  quickly  using  a  time  limited  branch  and  bound  search. 

The  success  of  the  additional  costs  suggests  additional 
research  into  other  desired  constraints  that  may  vary  between 
different  scenarios  and  objectives.  The  combinatorial  nature 
of  the  problem  allows  for  a  large  amount  of  near  optimal 
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